%% 目标固定在一点，改变测向站高度，优化三个测向站位置
clc
clear
close all

T =  [0 10 0];
lb = [-100 -100 -100 -100 -100 -100];
ub = [100 100 100 100 100 100];
% 测向站高度(km)
H1 = 40;
H2 = 30;
H3 = 20;
H4 = 10;
H5 = 5;
fun1 = @(S)SingleGdop(T,S,H1);
fun2 = @(S)SingleGdop(T,S,H2);
fun3 = @(S)SingleGdop(T,S,H3);
fun4 = @(S)SingleGdop(T,S,H4);
fun5 = @(S)SingleGdop(T,S,H5);
A = [];
b = [];
Aeq = [];
beq = [];

%% SQP
alpha1 = 5*pi/180;
alpha2 = alpha1+100*pi/180;
alpha3 = alpha2+100*pi/180;
R = 10;
S10 = [T(1)+R*cos(alpha1) T(2)+R*sin(alpha1)];
S20 = [T(1)+R*cos(alpha2) T(2)+R*sin(alpha2)];
S30 = [T(1)+R*cos(alpha3) T(2)+R*sin(alpha3)];
S0 = [S10 S20 S30];
nonlcon = [];
options = optimset('Display','iter','Algorithm','sqp');
[Smin1,fval1] = fmincon(fun1,S0,A,b,Aeq,beq,lb,ub,nonlcon,options);
[Smin2,fval2] = fmincon(fun2,S0,A,b,Aeq,beq,lb,ub,nonlcon,options);
[Smin3,fval3] = fmincon(fun3,S0,A,b,Aeq,beq,lb,ub,nonlcon,options);
[Smin4,fval4] = fmincon(fun4,S0,A,b,Aeq,beq,lb,ub,nonlcon,options);
[Smin5,fval5] = fmincon(fun5,S0,A,b,Aeq,beq,lb,ub,nonlcon,options);
figure
plot(T(1),T(2),'*','LineWidth',2);hold;
% plot([S0(1) S0(3) S0(5)],[S0(2) S0(4) S0(6)],'x','LineWidth',2);
plot([Smin1(1) Smin1(3) Smin1(5)],...
[Smin1(2) Smin1(4) Smin1(6)],'o','LineWidth',2);
plot([Smin2(1) Smin2(3) Smin2(5)],...
[Smin2(2) Smin2(4) Smin2(6)],'o','LineWidth',2);
plot([Smin3(1) Smin3(3) Smin3(5)],...
[Smin3(2) Smin3(4) Smin3(6)],'o','LineWidth',2);
plot([Smin4(1) Smin4(3) Smin4(5)],...
[Smin4(2) Smin4(4) Smin4(6)],'o','LineWidth',2);
plot([Smin5(1) Smin5(3) Smin5(5)],...
[Smin5(2) Smin5(4) Smin5(6)],'o','LineWidth',2);
legend('目标','H=40km','H=30km','H=20km','H=10km','H=5km','FontSize',12);
% title(['GDOP_m_i_n = ',num2str(fval)]);
xlabel('X/km');ylabel('Y/km');

%% GA
% [S_min,fval] = ga(fun,6,A,b,Aeq,beq,lb,ub);
% 
% figure
% plot(T(1),T(2),'*','LineWidth',2);hold;
% plot([S_min(1) S_min(3) S_min(5)],...
% [S_min(2) S_min(4) S_min(6)],'o','LineWidth',2);hold;
% title(['GDOP_m_i_n = ',num2str(fval)]);
% xlabel('X/km');ylabel('Y/km');
% rectangle('Position',[lb(1) lb(2) ub(1)-lb(1) ub(2)-lb(2)],...
% 'EdgeColor','k', 'LineWidth',2);
% axis([-100 100 -100 100])